#include "detector.hpp"


bool Detector::isPointInsideImage(const cv::Point2f& pt, const cv::Mat& image) {
	return pt.x > 10 && pt.x < image.cols - 10 && pt.y > 10 && pt.y < image.rows - 10;
}


// �������������ļнǣ���λ���ȣ�
double Detector::calculateAngle(const cv::Point2f& pt1, const cv::Point2f& pt2, const cv::Point2f& pt3) {
	// ��������
	cv::Point2f vec1 = pt1 - pt2;
	cv::Point2f vec2 = pt3 - pt2;

	// ������
	double dot = vec1.x * vec2.x + vec1.y * vec2.y;

	// ����������ģ
	double norm1 = sqrt(vec1.x * vec1.x + vec1.y * vec1.y);
	double norm2 = sqrt(vec2.x * vec2.x + vec2.y * vec2.y);

	// ����нǣ����ȣ�
	double angle = acos(dot / (norm1 * norm2));

	// ת��Ϊ�Ƕ�
	return angle * 180 / CV_PI;
}


cv::Mat Detector::PorcessImage(cv::Mat img) {
	cv::Mat gray_img, binary_img;
	cv::cvtColor(img, gray_img, cv::COLOR_BGR2GRAY);
	GaussianBlur(gray_img, gray_img, cv::Size(3, 3), 0);  //��˹�˲�
	threshold(gray_img, binary_img, 230, 255, cv::THRESH_BINARY);

	cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));
	//���и�ʴ����
	erode(binary_img, binary_img, element);
	//����ԭͼ 

	dilate(binary_img, binary_img, element);

	return binary_img;

}


std::vector<std::vector<cv::Point2f>> Detector::GetTriangles(cv::Mat binary_img)
{

	triangles.clear();

	std::vector<std::vector<cv::Point>> contours;
	std::vector < cv:: Vec4i > hierarchy;
	findContours(binary_img, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE);

	for (auto contour : contours)
	{
		//��ȡ��С��Ͼ�����ϣ���ת�Ƕ�̫����ʶ��
		auto b_rect = cv::boundingRect(contour);
		auto min_rect = minAreaRect(contour);

		//��ȡ͹����Ϻ����������
		std::vector<cv::Point> hull;
		convexHull(contour, hull);
		std::vector<cv::Point2f> triangle;

		//��ȡ��Ӧroi�����������ص��ж���
		cv::Mat mask = binary_img(b_rect);
		std::vector<cv::Point> points;
		cv::findNonZero(mask, points);

		//��ȡ��������������͹�����
		double AreaTriangle = minEnclosingTriangle(hull, triangle);
		double AreaHull = contourArea(hull);

		if (triangle.size() <= 0)
		{
			continue;
		}

		//�ж�����:
		//1.�ж��Ƿ��ǵ���ֱ��������
		//2.��͹���������̫��Ĳ�Ҫ��̫С��Ҳ��Ҫ
		//3.�����ص�������͹������ı���������̫��
		//4.͹����ϵ�������̫��
		//5.��ȡ��С��Ͼ�����ϣ���ת�Ƕ�̫����ʶ��
		//6.������ϵ㳬�߽�
		if (isRightIsoscelesTriangle(triangle, AreaHull, AreaTriangle) &&
			(AreaHull < max_AreaHull && AreaHull > min_AreaHull) &&
			points.size() / AreaHull > min_point_hull_ratio &&
			hull.size() >= min_hull_size &&
			min_rect.size.area() / b_rect.area() > min_Area_rect_ratio &&
			(isPointInsideImage(triangle[0], binary_img) && isPointInsideImage(triangle[1], binary_img) &&
				isPointInsideImage(triangle[2], binary_img)))
		{

			triangles.push_back(triangle);

		}



	}

	return triangles;


}




bool Detector::isRightIsoscelesTriangle(std::vector<cv::Point2f> &triangle,
	double areaHull, double areaTriangle) {
	// �������߳���
	const auto& a = triangle[0], b = triangle[1], c = triangle[2];
	const std::array<double, 3> edges = {
		cv::norm(a - b),
		cv::norm(b - c),
		cv::norm(c - a)
	};

	// Ѱ����ߣ�����Ϊб�ߣ������Ӧ��������
	const auto max_it = std::max_element(edges.begin(), edges.end());
	const int hypotenuse_idx = std::distance(edges.begin(), max_it);
	const int vertex_idx = (hypotenuse_idx + 2) % 3;  // ����ֱ�Ƕ�������

	// ��������˳�򣨽�ֱ�Ƕ����������1λ�ã�
	std::iter_swap(triangle.begin() + 1, triangle.begin() + vertex_idx);
	if (vertex_idx == 0) {  // ���������������˳ʱ��˳��
		std::swap(triangle[0], triangle[2]);
	}

	// ���»�ȡ������Ķ���
	 auto& p1 = triangle[0], & p2 = triangle[1], & p3 = triangle[2];


	// ����ֱ�ǽǶ�
	 double angle = calculateAngle(p1, p2, p3);

	// ��ȡ�����̱ߣ�������
	 double shorter1 = std::min({ edges[0],edges[1], edges[2]});
	 double shorter2 = std::max({ std::min(edges[0], edges[1]), std::min(edges[1], edges[2]), std::min(edges[0], edges[2]) });

	// ��֤����
	 bool valid_angle = (angle > min_angle) && (angle < max_angle);
	 bool valid_ratio = (shorter1 / shorter2 > min_side_ratio);
	 bool valid_pythagoras = std::abs(shorter1 * shorter1 + shorter2 * shorter2 -
		*max_it * *max_it) < max_Pythagorean_diff;
	 bool valid_area = (areaHull / areaTriangle > min_Area_hull_traingle_ratio);

	return valid_angle && valid_ratio && valid_pythagoras && valid_area;
}


void Detector::DrawImage(cv::Mat& img) {

	for (auto triangle: triangles){

		line(img, triangle[0], triangle[1], cv::Scalar(0, 0, 255), 2);
		line(img, triangle[1], triangle[2], cv::Scalar(0, 0, 255), 2);
		line(img, triangle[2], triangle[0], cv::Scalar(0, 0, 255), 2);

	
	}

	line(img, targets_[0], targets_[2], cv::Scalar(0, 0, 255), 2);
	line(img, targets_[1], targets_[3], cv::Scalar(0, 0, 255), 2);

}


bool Detector::select_final_targets(std::vector<cv::Point2f>& t) {

	targets_.clear();
	t.clear();

	if (triangles.size() != 4) { return false;}
	
	cv::Point2f targets[4];
	double min_area = 999999999999;
	int index= -1;

	for (int i = 0; i < 4; i++)
	{
		targets[i] = cv::Point2f(-1, -1);
		if (cv::contourArea(triangles[i]) < min_area)
		{
			index = i;
			targets[0] = triangles[i][1];
			min_area = cv::contourArea(triangles[i]);
		}

	}
	
	double center_x = (triangles[0][1].x + triangles[1][1].x + triangles[2][1].x + triangles[3][1].x) / 4.0;
	double center_y = (triangles[0][1].y + triangles[1][1].y + triangles[2][1].y + triangles[3][1].y) / 4.0;
	cv::Point2f center(center_x, center_y);

	// ����targets[0]��������ĵ������
	cv::Point2f v1(targets[0].x - center.x, targets[0].y - center.y);
	double theta1 = std::atan2(-v1.y, v1.x); // ת��Ϊ��׼����ϵ

	// ��������������
	for (int i = 0; i < 4; ++i) {
		if (i == index) continue;

		cv::Point2f current_point = triangles[i][1];
		cv::Point2f v2(current_point.x - center.x, current_point.y - center.y);
		double theta2 = std::atan2(-v2.y, v2.x); // ת��Ϊ��׼����ϵ

		// ����ǶȲ������[-��, ��]
		double delta_theta = theta2 - theta1;
		delta_theta = std::fmod(delta_theta + CV_PI, 2 * CV_PI) - CV_PI;

		// ת��Ϊ˳ʱ��Ϊ���ĽǶ�
		double angle = -delta_theta;
	
	
		// ������Ȼ�ת��Ϊ������ʾ����
		double angle_deg = angle * 180.0 / CV_PI;
		std::cout << angle_deg << std::endl;
		// �洢��ʹ�øýǶ�
		if (angle_deg > 40 && angle_deg < 120)
		{
			targets[1]  = triangles[i][1];
		}
		if ((angle_deg >= 120 && angle_deg <= 190)||(angle_deg >= -130 && angle_deg <= -180))
		{
			
			targets[2] = triangles[i][1];
		}
		if (angle_deg > -130 && angle_deg < -40)
		{
	
			targets[3] = triangles[i][1];
		}
	}

	if (targets[0].x != -1 &&
		targets[1].x != -1 &&
		targets[2].x != -1 &&
		targets[3].x != -1)
	{
		std::copy(targets, targets + 4, std::back_inserter(t));
		std::copy(targets, targets + 4, std::back_inserter(targets_));
		return true;
	}

	return false;

}